/* 注意，下位机使用的坐标系是Y朝前，而上位机和一般机器人中都为X朝前，相关转化在本文件中解决 */
#include "AsyncSerial.h"
#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/Float32MultiArray.h>

using namespace std;

class LowerDriver {

public:
    LowerDriver();

private:
    enum{ LOOP_RATE=20, };  // const value

    ros::NodeHandle nh;
    ros::Publisher odom_pubber;
    ros::Publisher voltage_pubber;
    nav_msgs::Odometry odom;
    float voltage1, voltage2;
    CallbackAsyncSerial * pSerial;

    void SerialHandler(const char data[], unsigned int len);
    void CmdVelCb(const geometry_msgs::Twist &command);
};

LowerDriver::LowerDriver(){
    // 参数服务器读取参数 
    string port, cmd_topic;
    ros::param::param<std::string>("~port", port, "/dev/ttyUSB0");
    ros::param::param<std::string>("~cmd_topic", cmd_topic, "cmd_vel");
    // 订阅/发布
    ros::Subscriber vel_subber = nh.subscribe(cmd_topic, 1, &LowerDriver::CmdVelCb, this);
    odom_pubber = nh.advertise<nav_msgs::Odometry>("odom", 1);
    voltage_pubber = nh.advertise<std_msgs::Float32MultiArray>("voltages", 1);
    // 初始化odom一些参数
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
    try {
        cout<<"open port:"<<port<<" baudrate:"<<115200<<endl;
        CallbackAsyncSerial serial(port, 115200);
		pSerial = &serial;
        serial.setCallback(boost::bind(&LowerDriver::SerialHandler, this, _1,_2));
        ros::Rate r(LOOP_RATE);  // [HZ]
        while (ros::ok())
        {
            // 发布odom
            ros::Time current_time;
            odom.header.stamp = current_time.fromSec(ros::Time::now().toSec());
            odom_pubber.publish(odom);
            // 发布tf
            static tf::TransformBroadcaster tfbr;
			tf::Quaternion q(0,0,odom.pose.pose.orientation.z,odom.pose.pose.orientation.w);
            tf::Transform transform;
            transform.setOrigin( tf::Vector3(odom.pose.pose.position.x, odom.pose.pose.position.y, 0.0) );
            transform.setRotation(q);
            tfbr.sendTransform(tf::StampedTransform(transform, current_time.fromSec(ros::Time::now().toSec()), "odom", "base_link"));
            // 发布电压信息
            std_msgs::Float32MultiArray voltages;
            voltages.data.push_back(voltage1);
            voltages.data.push_back(voltage2);
            voltage_pubber.publish(voltages);
            // 周期循环
            if(serial.errorStatus() || serial.isOpen()==false)
            {
                ROS_ERROR_STREAM("Error: serial port closed unexpectedly");
                break;
            }
            ros::spinOnce();
            r.sleep();
        }
        serial.close();
    } catch (std::exception& e) {
        ROS_ERROR_STREAM("Open serial port " + port + " failed.");
        ROS_ERROR_STREAM("Exception: " << e.what());
    }
    ros::shutdown();
}

// 下位机上发协议解析,更新odom
void LowerDriver::SerialHandler(const char data[], unsigned int len){
    if(data[0]!=0x65)return;      // 若帧头不对
    char func = data[1];
    char data_len = data[2];
    if(data_len != len-4)return;  // 若总长度 != 数据位长度+4
    char sum = 0;
    for(int i=3;i<len-1;i++)sum += data[i];  // 校验位计算
    if(sum != data[len-1])return;  // 若校验不通过
    switch(func){  // 校验通过 根据功能码执行功能
        case 0x01:
        {
            float low_x = *(float *)(data+3);
            float low_y = *(float *)(data+7);
            odom.twist.twist.linear.x = low_y;
            odom.twist.twist.linear.y = -low_x;
            odom.twist.twist.angular.z = *(float *)(data+11);
            break;
        }
        case 0x02:
        {
            float low_x = *(float *)(data+3);
            float low_y = *(float *)(data+7);
            odom.pose.pose.position.x = low_y;
            odom.pose.pose.position.y = -low_x;
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(*(float *)(data+11));
            odom.pose.pose.orientation = odom_quat;
            break;
        }
        case 0x04:
        {
            voltage1 = *(float *)(data+3);
            voltage2 = *(float *)(data+7);
        }
    }
}

// cmd_vel话题回调，向下位机下发速度指令
void LowerDriver::CmdVelCb(const geometry_msgs::Twist &command){
    float up_vx = command.linear.x;
    float up_vy = command.linear.y;
    float omega = command.angular.z;
    float vx = -up_vy;
    float vy = up_vx;
    float databuf[3] = {vx, vy, omega};  // 用来memcpy
    // 线速度/角速度
    char cmd[16];
    cmd[0] = 0x65;
    cmd[1] = 0x11;
    cmd[2] = 0x0C;
    cmd[15] = 0x00;
    memcpy(cmd+3, databuf, 12);
    for(char i=3;i<15;i++)cmd[15]+=cmd[i];  // 校验位计算
    pSerial->write(cmd, 16);
}

int main(int argc, char **argv)
{
    // ROS初始化
    ros::init(argc, argv, "lower_driver");
    LowerDriver lowerDriver;
}
